/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

// 
// This file test the command object and its functions
//
//

#include <stdlib.h>
#include <stdio.h>
#include <iostream>

typedef unsigned char UCHAR;
typedef unsigned short UINT16;
typedef unsigned int UINT32;

#include "command.h"

int main(int argc, char *argv[])
{
	Command cmd;
	cout << "GPSTelem_t=" << sizeof( GPSTelem_t ) << endl;
	cout << "AirTelem_t=" << sizeof( AirTelem_t ) << endl;
	cout << "IMUAngles_t=" << sizeof( IMUAngles_t ) << endl;
	cout << "NavigationStatus_t=" << sizeof( NavigationStatus_t ) << endl;
	cout << "SysDiag_t=" << sizeof( SysDiag_t  ) << endl;
	cout << "FailureBits_t=" << sizeof( FailureBits_t  ) << endl;
	cout << "Limits_t=" << sizeof( Limits_t ) << endl;
	cout << "sizeof(Telemetry_t)= " << sizeof(Telemetry_t) << endl;

	// to simple test and print
	//cmd.to(1);
	cmd.cmd(2);
	cmd.num(3);
	UCHAR parms[] = {4,5,6};
	cmd.setParams(parms, 3);
	cmd.printCmd();

	// test checksum
	cout << "cmd.isValid = " << cmd.isValid() << endl;
	// set and recheck checksum
	cmd.setCheckSum();
	cout << "cmd.isValid = " << cmd.isValid() << endl;

	// do some buffer tests
	UCHAR buf[] = { 1, 2, 3, 4, 5, 6, 7, 8};
	UCHAR buf1[] = { 0, 0, 0, 0, 0, 0, 0, 0};

	// construc from buffer
	Command cmd1(buf);
	cmd1.printCmd();
	cout << "cmd1.isValid = " << cmd1.isValid() << endl;
	cmd1.setCheckSum();
	cout << "after cmd1.setCheckSum cmd1.isValid = " << cmd1.isValid() << endl;

	// fill from buffer
	Command cmd2;
	cmd2.copyFromBuf(buf);	
	cout << "cmd2.isValid = " << cmd2.isValid() << endl;
	cmd2.setCheckSum();
	cout << "after cmd2.setCheckSum cmd2.isValid = " << cmd2.isValid() << endl;
	cmd2.printCmd();

	// fill to buffer
	cmd2.copyToBuf(buf1);	
	for(int i=0; i < cmd2.size(); i++)
		printf("\t0x%x", buf1[i]);
	cout << endl;
	
	return EXIT_SUCCESS;
}
